#ifndef OMPL_BASE_TRAJECTORY_EVALUATOR_
#define OMPL_BASE_TRAJECTORY_EVALUATOR_

#include "ompl/base/State.h"
#include "ompl/base/SpaceInformation.h"
#include "ompl/util/ClassForward.h"
#include <planning_environment/models/collision_models_interface.h>
#include <planning_models/kinematic_model.h>
#include <ompl_ros_interface_helper/ompl_ros_conversions.h>
#include <Eigen/Core>
#include <kdl/tree.hpp>
#include <kdl/chain.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <ros/ros.h>


namespace ompl
{
  namespace base
  {

    ClassForward(TrajectoryEvaluator);

    class TrajectoryEvaluator
    {
    public:
      TrajectoryEvaluator() {}
      virtual ~TrajectoryEvaluator() {}
      virtual double getCost(const std::vector<State*>& trajectory) const = 0;
    };



    static const int DIFF_RULE_LENGTH = 7;
    

    static const double DIFF_RULES[3][DIFF_RULE_LENGTH] = {
                                                           {0, 0, -2/6.0, -3/6.0, 6/6.0, -1/6.0, 0},                   // velocity
                                                           {0, -1/12.0, 16/12.0, -30/12.0, 16/12.0, -1/12.0, 0},       // acceleration
                                                           {0, 1/12.0, -17/12.0, 46/12.0, -46/12.0, 17/12.0, -1/12.0}  // jerk

    };

    class SmoothnessEvaluator : public TrajectoryEvaluator
    {
    public:
      SmoothnessEvaluator(ompl::base::SpaceInformation* si,
                          planning_environment::CollisionModelsInterface* cmi,
                          const std::string& group_name,
                          double discretization = 0.03,
                          double smoothness_cost_velocity = 0.0,
                          double smoothness_cost_acceleration = 1.0,
                          double smoothness_cost_jerk = 0.0);

      double getCost(const std::vector<State*>& trajectory) const;


    private:
      double discretization_;
      double smoothness_cost_velocity_;
      double smoothness_cost_acceleration_;
      double smoothness_cost_jerk_;

      ompl::base::SpaceInformation* si_;
      planning_environment::CollisionModelsInterface* cmi_;

      const planning_models::KinematicModel::JointModelGroup* joint_group_;

      ompl_ros_interface_helper::OmplStateToKinematicStateMapping ompl_state_to_kinematic_state_mapping_;
      ompl_ros_interface_helper::KinematicStateToOmplStateMapping kinematic_state_to_ompl_state_mapping_;



      Eigen::MatrixXd getDiffMatrix(int size, const double* diff_rule) const;
    };

    class DynamicsEvaluator : public TrajectoryEvaluator
    {
      typedef planning_models::KinematicModel::JointModel JointModel;
    public:
      DynamicsEvaluator(ompl::base::SpaceInformation* si,
                        planning_environment::CollisionModelsInterface* cmi,
                        const std::string& group_name,
                        double discretization = 0.3);

      bool calculateKDLChain();


      double getCost(const std::vector<State*>& trajectory) const;

      void getTorques(int traj_point,
                      const std::vector<std::vector<double> >& joint_state_values,
                      const std::vector<KDL::Wrench>& fext,
                      std::vector<double>& torques) const;

      void getJointPositions(int traj_point,
                             const std::vector<std::vector<double> >& joint_state_values,
                             KDL::JntArray& kdl_jnt_positions) const;

      void getJointVelocities(int traj_point,
                              const std::vector<std::vector<double> >& joint_state_values,
                              KDL::JntArray& kdl_jnt_velocities) const;

      void getJointAccelerations(int traj_point,
                                 const std::vector<std::vector<double> >& joint_state_values,
                                 KDL::JntArray& kdl_jnt_accelerations) const;


    private:

      ompl::base::SpaceInformation* si_;
      planning_environment::CollisionModelsInterface* cmi_;

      const planning_models::KinematicModel::JointModelGroup* joint_group_;

      ompl_ros_interface_helper::OmplStateToKinematicStateMapping ompl_state_to_kinematic_state_mapping_;
      ompl_ros_interface_helper::KinematicStateToOmplStateMapping kinematic_state_to_ompl_state_mapping_;

      boost::shared_ptr<KDL::ChainIdSolver> inverse_dynamics_solver_;
      KDL::Chain kdl_chain_;
      KDL::Tree kdl_tree_;

      double discretization_;

      std::map<int, int> kdl_joint_id_to_joint_id_mapping_;
      std::vector<std::string> kdl_joint_id_to_joint_name_mapping_;
    };
  }
}

#endif
